#ifndef Message_TASK_H
#define Message_TASK_H


#include "cmsis_os2.h"  // ::CMSIS:RTOS2
#include "string.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "app_preference.h"
#include "dev_can.h"
#include "app_serial.h"
#include "protocol_dbus.h"
#include "protocol_judgement.h"
#include "drivers_statistic.h"
#include "algorithm_Kalman_IMUCalc.h"
#include "MWMotor.h"
#include "app_motor.h"
#include "bsp_dwt.h"

#ifdef __cplusplus
extern "C" {
#endif

	void Message_Task(void *pvParameters);
	void CAN1_Rx_Task(void *pvParameters);
	void CAN2_Rx_Task(void *pvParameters);
	void CAN3_Rx_Task(void *pvParameters);
	void Serial_Rx_Task(void *pvParameters);
	void Referee_Rx_Task(void *pvParameters);
	void DR16_Rx_Task(void *pvParameters);

	
#ifdef __cplusplus
}
#endif


extern QueueHandle_t Message_Queue;   		//��Ϣ���о��
extern QueueHandle_t Message_Queue;
extern QueueHandle_t CAN1_Rx_Queue;
extern QueueHandle_t CAN2_Rx_Queue;
extern QueueHandle_t CAN3_Rx_Queue;
extern QueueHandle_t Serial_Rx_Queue;
extern QueueHandle_t Referee_Rx_Queue;
extern QueueHandle_t DR16_Rx_Queue;

#define CAP_CHECK 0x00//���״̬
#define CAP_CLOSE 0x0f//�ر�״̬
#define CAP_OPEN  0xf0//����״̬
#define CAP_ERROR 0xff//����״̬
#define CAP_MODE1 0x00//�رղ���
#define CAP_MODE2 0xff//��������

#define UIN16_MPU_RAD 182
#define FP32_MPU_RAD  0.005494505494505494//0.0000958772770853307766059
#define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f

#define ANGLE_TO_RAD        0.01745329251994329576923690768489
#define RAD_TO_ANGLE        57.295779513082320876798154814105
#define BMI088_ACCEL_6G_SEN 0.00179443359375f


union F
{
	uint8_t I[4];
	fp32 F;
};
union int16_t_uint8_t
{
    uint8_t uint_8[2];
    int16_t int_16;
};
union uint32_t_uint8_t
{
    uint8_t   uint_8[4];
    uint32_t  uint_32;
};


#pragma pack(1)
/*�Ӿ���������ݽṹ��*/
struct Visual_Receive_Data_t
{
	  uint8_t Len;
		uint8_t Header;
		bool  fire;
	  union F yaw;
	  union F pitch;
	  union F distance;
		uint8_t check_byte;
};

#pragma pack()

struct supercap_Receive_Data_t
{
    uint8_t situation;  //0x00�Լ� 0x0f�ر� 0xf0���� 0xff����
    uint8_t mode;       //0x00ģʽ1 0xffģʽ2
    float   power;
    uint8_t energy;
    uint8_t power_limit;
    uint8_t errorcode;

    uint8_t *ptr;
};

struct Gyro_RX_data_t
{
	fp32 Picth;
	fp32 Roll;
	fp32 Yaw;
	fp32 SpeedZ;
	fp32 SpeedX;
	fp32 SpeedY;
	fp32 Acce_Z;
	fp32 Acce_Y;
	
	fp32 Error_angle;
	fp32 Yaw_cycle;
	fp32 Last_YAW_angle;
	fp32 Yaw_angle;
	fp32 Yaw_speed;
	fp32 Pitch_angle;
	fp32 Pitch_speed;	
	fp32 Roll_angle;
	fp32 Roll_speed;		
};

#pragma pack(1)
typedef struct {
  fp32 x;
  fp32 y;
  fp32 z;
} Vector3;
#pragma pack()

#pragma pack()

#pragma pack(1)
typedef struct  {
  fp32 q0;
  fp32 q1;
  fp32 q2;
  fp32 q3;
} Quaternion;
#pragma pack()

#pragma pack(1)
typedef struct  {
  fp32 yaw;
  fp32 pit;
  fp32 rol;
} EulerAngles;
#pragma pack()


#pragma pack(1)
typedef struct  {
	uint8_t     len;
  uint8_t     prefix;
  uint8_t     id;
  uint32_t    time_ms;
	Quaternion  quat;
  Vector3     gyro;
	Vector3     accl;
  EulerAngles eulr;
  uint8_t     crc8;
} Data;
#pragma pack()

#pragma pack(1)
typedef struct  {
	uint8_t len;
	uint8_t Header;
	float   Yaw;
	float   Pitch;
	float   Roll;
	float   Speed_X;
	float   Speed_Y;
	float   Speed_Z;
	float   Acce_X;
	float   Acce_Y;
	float   Acce_Z;
}C_Gyro_data_t;
#pragma pack()


struct MPU_Data_tZ
{
			uint8_t HHH;
			uint8_t KEY;
	    int16_t_uint8_t AngleZ;
	    int16_t_uint8_t Speed_Z;
      int16_t_uint8_t Acce_Z;
      int16_t_uint8_t Acce_Y;
};

struct MPU_Data_tXY
{
			uint8_t HHH;
			uint8_t KEY;
			int16_t_uint8_t AngleX;
			int16_t_uint8_t Speed_X;
			int16_t_uint8_t AngleY;
			int16_t_uint8_t Speed_Y;
};

struct CAN_QD_IMU_Data
{
	float yaw;
	float pitch;
	float roll;
	float Speed_X;
	float Speed_Y;
	float Speed_Z;
};

class Message_Ctrl:public Statistic
{
public:
	
	float    Visual_Task_DWT_dt;
	uint32_t Visual_Task_DWT_Count;
  Visual_Receive_Data_t     VisualR;
  supercap_Receive_Data_t   SuperCapR;
	Gyro_RX_data_t     Gyro_data;


	CAN_QD_IMU_Data QDIMU_R;
  Data            U_QDIMU_R;
	C_Gyro_data_t 	C_Gyro_data;
	MPU_Data_tZ     MPU_DataZ;
	MPU_Data_tXY    MPU_DataXY;
	const judge_type_t   *robo;


	RC_ctrl_t *RC_Ptr;

	void Hook();
	void Init();
	void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
	void ToEulerAngles(fp32 q1, fp32 q2, fp32 q3, fp32 q4);
	void CAN1_Process(CanRxMsg *Rx_Message);
	void CAN2_Process(CanRxMsg *Rx_Message);
	void CAN3_Process(CanRxMsg *Rx_Message);
	void Serialx_Hook(uint8_t *Rx_Message, Serialctrl *Serialx_Ctrl);
	void Visual_Serial_Hook(uint8_t *Rx_Message);
	void Gyro_Chassis_Hook(uint8_t *Rx_Message);
	void Gyro_Gimbal_Serial_Hook(uint8_t *Rx_Message);

private:
	fp32 bullet_speed_last;
};

extern Message_Ctrl Message;
			 Message_Ctrl *get_message_ctrl_pointer(void);
		   RC_ctrl_t    *get_remote_control_point(void);


typedef struct
{
	uint8_t	key_flag;
	uint8_t  count;//����
	uint8_t  last_count;
}count_num_key;

typedef enum
{
	single = 0,
	even,
	count,
}key_count_e;

struct rc_key_v_t
{
	//����
	count_num_key W;
	count_num_key S;
	count_num_key A;
	count_num_key D;
	count_num_key shift;
	count_num_key ctrl;
	count_num_key Q;
	count_num_key E;
	count_num_key R;
	count_num_key F;
	count_num_key G;
	count_num_key Z;
	count_num_key X;
	count_num_key C;
	count_num_key V;
	count_num_key B;
};

struct rc_press_t
{
	//���
	count_num_key L;
	count_num_key R;
};

class rc_key_c
{
public:
	rc_key_v_t Key;
	rc_press_t Press;

	void rc_key_v_set(RC_ctrl_t *RC);
	uint8_t read_key(count_num_key *temp_count, key_count_e mode, bool clear);
	bool read_key(count_num_key *temp_count, key_count_e mode, bool *temp_bool);
	void clear_key_count(count_num_key *temp_count);
private:
	bool read_key_single(count_num_key *temp_count);
	bool read_key_single(count_num_key *temp_count, bool *temp_bool);
	bool read_key_even(count_num_key *temp_count);
	bool read_key_even(count_num_key *temp_count, bool *temp_bool);
	void sum_key_count(int16_t key_num, count_num_key *temp_count);
};


#endif
